#include <ros/ros.h>
#include <flydatas_msgs/Flydatas.h>
#include <sensor_msgs/NavSatFix.h>
#include <mavros_msgs/State.h>
#include <geometry_msgs/PoseStamped.h>
#include <std_msgs/Header.h>

ros::Publisher pub;
bool should_publish = true;

void flydatasCallback(const flydatas_msgs::Flydatas::ConstPtr& msg)
{
    // Process GPS data
    double gps_lat = msg->gps[0].latitude;
    double gps_lon = msg->gps[0].longitude;
    double gps_alt = msg->gps[0].altitude;
    ROS_INFO("callback_doit: %d", msg->doit);

    // Check the condition to stop publishing
    if (msg->doit == true)
    {
        ROS_INFO("should_publish will be false");
        should_publish = false;
        pub.shutdown();  // Unregister the publisher
        ros::shutdown();  // 关闭 ROS 节点
    }
}

void publishFlydatas()
{
    ros::Rate rate(10);  // 10 Hz

    flydatas_msgs::Flydatas msg;

    // Filling Header
    msg.header.stamp = ros::Time::now();
    msg.header.frame_id = "base_link";

    // Filling State (replace with actual values)
    msg.flight_state.mode = "OFFBOARD";
    msg.flight_state.armed = true;

    // Initial GPS values
    msg.gps[0].latitude = 21.679680;
    msg.gps[0].longitude = 110.919130;
    msg.gps[0].altitude = 17.8;

    msg.gps[1].latitude = 0;
    msg.gps[1].longitude = 0;
    msg.gps[1].altitude = 0;

    msg.gps[2].latitude = 0;
    msg.gps[2].longitude = 0;
    msg.gps[2].altitude = 0;

    msg.gps[3].latitude = 0;
    msg.gps[3].longitude = 0;
    msg.gps[3].altitude = 0;

    // Filling PoseStamped (replace with actual pose values)
    msg.pos.pose.position.x = 1.0;
    msg.pos.pose.position.y = 2.0;
    msg.pos.pose.position.z = 3.0;

    msg.task = 2.0;

    // Filling boolean flag
    msg.doit = true;

    while (ros::ok())
    {
        msg.header.stamp = ros::Time::now();  // Update timestamp

        if (should_publish == true)
        {
            pub.publish(msg);
            ROS_INFO("lat: %f, lon: %f, alt: %f", msg.gps[0].latitude, msg.gps[0].longitude, msg.gps[0].altitude);
        }
        else
        {
            ROS_INFO("stop to publish");
            break;
        }
        ros::spinOnce();
        rate.sleep();
    }
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "flydatas_pub_node");
    ros::NodeHandle nh;

    // Initialize the publisher
    pub = nh.advertise<flydatas_msgs::Flydatas>("/pub_flydatas", 10);

    // Subscribe to the /sub_flydatas topic
    ros::Subscriber sub = nh.subscribe("/sub_flydatas", 10, flydatasCallback);

    // Call the publishFlydatas function
    publishFlydatas();

    return 0;
}
